#include "SIFTMatcher.hpp"
#ifdef ZZZ_LIB_ANN_CHAR
#include <boost/lambda/lambda.hpp>
#include <Utility/CheckPoint.hpp>
#include <Utility/CmdParser.hpp>
using namespace boost::lambda;

namespace zzz{
ZFLAGS_INT(ann_max_visit_pt, 200, "Maximum visit point number for each ann search");

SIFTMatcher::SIFTMatcher(){}

SIFTMatcher::SIFTMatcher(const SIFTKeys &keys)
{
  Prepair(keys);
}

void SIFTMatcher::Prepair(const SIFTKeys &keys)
{
  keys1n_=keys.desc_.size();
  ann_.SetData(keys.desc_);
}

void SIFTMatcher::Match(const SIFTKeys &keys)
{
  //use matching strategy described in Photo Tourism
  vector<int> dists(keys1n_,-1);
  vector<int> key12(keys1n_,-1);
  vector<int> key21(keys.keys_.size(),-1);
  vector<int> idx;
  vector<int> dist;
  ann_.SetMaxPointVisit(ZFLAG_ann_max_visit_pt); //this will increase the search speed severly
  for (zuint i=0; i<keys.keys_.size(); i++)
  {
    ann_.Query(keys.desc_[i],2,idx,dist);
    if (dist[0]>=0.6*0.6*dist[1]) continue;  //only if the first match is far better than the second one
    if (key12[idx[0]]==-1 && key21[i]==-1)
    {
      key12[idx[0]]=i;
      key21[i]=idx[0];
      dists[idx[0]]=dist[0];
    }
    else  //if some match happens before, none of them can count as a match!
    {
      key12[idx[0]]=-2;
      key21[i]=-2;
    }
  }

  //write to vector
  matches_.clear();
  matches_.reserve(keys1n_);
  dists_.clear();
  dists_.reserve(keys1n_);
  for (zuint i=0; i<keys1n_; i++)
  {
    if (key12[i]>=0)
    {
      matches_.push_back(make_pair(i,key12[i]));
      dists_.push_back(dists[i]);
    }
  }
}

void SIFTMatcher::Sort()
{
  vector<pair<double,int> > nodes;
  for (zuint i=0; i<dists_.size(); i++)
    nodes.push_back(make_pair(dists_[i],i));
  sort(nodes.begin(),nodes.end());
  vector<pair<zuint,zuint> > new_matches;
  vector<double> new_dists;
  for (zuint i=0; i<nodes.size(); i++)
  {
    new_matches.push_back(matches_[nodes[i].second]);
    new_dists.push_back(dists_[nodes[i].second]);
  }
  matches_=new_matches;
  dists_=new_dists;
}

}

#endif // ZZZ_LIB_ANN_CHAR